#include <chrono>
#include <functional>
#include <memory>
#include <string>
#include<vector>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float32_multi_array.hpp"
#include "serial/serial.h"

#define DEBUD_MODE 0          // 调试信息开关宏定义

using namespace std::chrono_literals;
using namespace std;

/* This example creates a subclass of Node and uses std::bind() to register a
* member function as a callback from the timer. */

#define PKG_SIZE    (97)    // 数据包大小

serial::Serial nav_ser;     // 定义串口对象
struct NAV_DATA             // 导航数据结构体
{
  float tim, wib[3], fsf[3], mag[3], pre[2], att[3];
};
struct NAV_DATA nav_data;   // 定义导航数据

class NavpiePublisher : public rclcpp::Node         // 导航信息发布者
{
  public:
    NavpiePublisher()
    : Node("navpie_publisher"), count_(0)
    {
      publisher_ = this->create_publisher<std_msgs::msg::Float32MultiArray>("navpie", 10);
      timer_ = this->create_wall_timer(5ms, std::bind(&NavpiePublisher::timer_callback, this));
    }

  private:
    unsigned char reacvdata[PKG_SIZE]={0};       // 数据包
    unsigned char data, flag_imu_recv = false;
    void timer_callback()
    {
      nav_ser.read(&data, 1);
      if (data == 0xEB) {
        nav_ser.read(&data, 1);
        if (data == 0x90) {
            nav_ser.read(reacvdata, PKG_SIZE-2);
            nav_data.tim = *((float*)&reacvdata[0]);
            for (int i = 0; i < 3; i++) {
                nav_data.wib[i] = *((float*)&reacvdata[4 + i * 4]);
                nav_data.fsf[i] = *((float*)&reacvdata[16 + i * 4]);
                nav_data.mag[i] = *((float*)&reacvdata[60 + i*4]);
                nav_data.att[i] = *((float*)&reacvdata[82 + i * 4]);
            }
            for (int i = 0; i < 2; i++) {
                nav_data.pre[i] = *((float*)&reacvdata[73 + i * 4]);
            }
            flag_imu_recv = true;
        }
      }

      if (flag_imu_recv) {
        auto message = std_msgs::msg::Float32MultiArray();
        vector<float> nav_data_vec;
        nav_data_vec.push_back(nav_data.tim);
        nav_data_vec.push_back(nav_data.wib[0]);
        nav_data_vec.push_back(nav_data.wib[1]);
        nav_data_vec.push_back(nav_data.wib[2]);
        nav_data_vec.push_back(nav_data.fsf[0]);
        nav_data_vec.push_back(nav_data.fsf[1]);
        nav_data_vec.push_back(nav_data.fsf[2]);
        nav_data_vec.push_back(nav_data.mag[0]);
        nav_data_vec.push_back(nav_data.mag[1]);
        nav_data_vec.push_back(nav_data.mag[2]);
        nav_data_vec.push_back(nav_data.pre[0]);
        nav_data_vec.push_back(nav_data.pre[1]);
        nav_data_vec.push_back(nav_data.att[0]);
        nav_data_vec.push_back(nav_data.att[1]);
        nav_data_vec.push_back(nav_data.att[2]);
        message.data = nav_data_vec;
#if DEBUD_MODE==1
        RCLCPP_INFO(this->get_logger(), "%6.2f\t%6.3f %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f", nav_data.tim,
                    nav_data.wib[0],nav_data.wib[1],nav_data.wib[2], nav_data.fsf[0],nav_data.fsf[1],nav_data.fsf[2],
                    nav_data.att[0],nav_data.att[1],nav_data.att[2]);
#endif
        publisher_->publish(message);
        flag_imu_recv = false;
      }
    }
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr publisher_;
    size_t count_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);

  nav_ser.setPort("/dev/ttyUSB0");
  nav_ser.setBaudrate(460800);
  serial::Timeout to =serial::Timeout::simpleTimeout(1000);
  nav_ser.setTimeout(to);
  nav_ser.open();
  if(nav_ser.isOpen()) {
    RCLCPP_INFO("连接到导航模块");
  } else {
    RCLCPP_INFO("连接失败");
  }
  
  rclcpp::spin(std::make_shared<NavpiePublisher>());
  rclcpp::shutdown();
  return 0;
}